package frc.circuitrunners.RobotSubsystems.DriveTrain;

/** Imports */
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import frc.circuitrunners.RobotSubsystems.RobotSubsystem;
import edu.wpi.first.wpilibj.*;
import frc.circuitrunners.IO.IO;
import frc.circuitrunners.Scheduler;


/**
 * @author CircuitRunners
 * This package and class contain all implementation of the Drive code
 */
public class Drive extends RobotSubsystem {
    
    /**
     * Jaguars that power the drive train
     */
    private CANJaguar FrontRight, FrontLeft, BackRight, BackLeft;
    private Encoder FrontRightE, FrontLeftE, BackRightE, BackLeftE;
    private ADXL345_I2C robotAccelerometer;
    private Vector acceleration, velocity, position;
    private static final double INCHES_PER_ROTATION = 6.0 * Math.PI;
    /**
     * Run a stage of the autonomous process
     * @param stage the stage of the autonomous process to run
     */
    public void run(int stage) {
        switch(stage){
            case 1:
                setFlag(true);
                break;
            case 2:
                //put stage 2 code here, don't use move method. 
                setFlag(true);
                break;
            case 3:
                while(getFlag() == false){
                Vector stage3move = new Vector(.7,270);
                this.directionalMove(stage3move);
                //set stopping condition for encoders
                }
            case 4:
                setFlag(true);
                
        }
    }
    
    /**
     * Initializing the drivetrain
     */
    public void init() {
        Scheduler.feed();
        try {   
            FrontRight = new CANJaguar(1); //Initialize front right motor
            FrontLeft = new CANJaguar(2); //Initialzie front left motor
            BackRight = new CANJaguar(3); //Initialize back right motor
            BackLeft = new CANJaguar(4); //Initialize back left motor
            FrontRight.setVoltageRampRate(0.0);
            FrontLeft.setVoltageRampRate(0.0);
            BackRight.setVoltageRampRate(0.0);
            BackLeft.setVoltageRampRate(0.0);
            robotAccelerometer = new ADXL345_I2C(1,ADXL345_I2C.DataFormat_Range.k2G);
            //robotGyro = new Gyro(2,1); //Initialize Gyro
        } catch (CANTimeoutException GGYOUFAIL) {
            Scheduler.yield();
        }
    }

    /**
     * Run the Teleop code written here for the drivetrain
     */
    public void runTeleop() {
        Scheduler.feed();
        try {
            double x, y2, y1; //variables to store joystick input
            x = limit(IO.getInput(IO.JOY_RIGHT | IO.X) + IO.getInput(IO.JOY_LEFT | IO.X)); //Combined X Axis Input
            y2 = IO.getInput(IO.JOY_RIGHT | IO.Y); //Right Y Axis input 
            y1 = IO.getInput(IO.JOY_LEFT | IO.Y); //Left Y Axis input
            double[] motorspeeds = new double[4]; //Array of normalization of motion
            motorspeeds[0] = y2 - x; //Front Right
            motorspeeds[1] = -(y1 + x); //Front Left
            motorspeeds[2] = y2 + x; //Back Right
            motorspeeds[3] = -(y1 - x); //Back Left
            double highest = 0.0, temp = 0.0; //Normalization temp variables
            for(int i = 0; i < motorspeeds.length; i++){ //run through the all motor input speeds
                temp = Math.abs(motorspeeds[i]); //Absolute value of the movement
                if(temp > highest) //finds highest value of all motor speeds...
                    highest = temp; //...and stores it into the highest variable
            }
            if(highest > 1.0){ //If the highest value is greater than 1.0
                for(int i = 0; i < motorspeeds.length; i++){ //Divide all motorspeeds by th highest motor speed
                    motorspeeds[i] = motorspeeds[i] / highest;
                }
            }
            FrontRight.setX(motorspeeds[0]); //set front right motor to normalized calculated speed
            FrontLeft.setX(motorspeeds[1]); //set front left motor to normalized calculated speed
            BackRight.setX(motorspeeds[2]); //set back right motor to normalized calculated speed
            BackLeft.setX(motorspeeds[3]); //set back left motor to normalized calculated speed
        } catch (CANTimeoutException ex) {
            Scheduler.yield();
        }
    }
    
    /**
     * Limits our input or output to the normalized -1.0 to 1.0
     * @param input the input
     * @return the limited output
     */
    private double limit(double input){
        if(input >= 1.0){ //if greater than 1.0
            return 1.0; //return 1.0
        } else if(input <= -1.0){ //if less than -1.0
            return -1.0; //return -1.0
        } else { //otherwise
            return input; //return the input
        }
    }
    
    /**
     * Move the robot to x,y coordinate
     * @param x the x coordinate 
     * @param y the y coordinate
     * @param speed the speed which the robot shall move. between -1.0 and 1.0.
     */
    public void move(double x, double y, double speed){
        
    }
    
    /**
     * Move in a straight line at an angle theta from the horizontal
     * @param Vector v the movement vector of the robot
     * Note, this is not a robot heading!!!! This is for moving diagonally or in any direction
     */
    public void directionalMove(Vector V){
        try {
            double velocity = V.getMagnitude();
            double xVelocity = V.getXComponent();
            double yVelocity = V.getYComponent();
            
            FrontRight.setX(yVelocity - xVelocity);
            FrontLeft.setX(-(yVelocity + xVelocity));
            BackRight.setX(yVelocity + xVelocity);
            BackLeft.setX(-(yVelocity - xVelocity));
    }
        catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }    
    }
    /**
     * Drive completely stops when auto aiming
     */
    public void autoAim() {
        disable(); //disables drive train when auto aiming
    }
    
    /**
     * Stops robot completely when disabled
     */
    public void disable(){
        Scheduler.feed();
        try {  
            FrontRight.setX(0); //stops front right motor
            FrontLeft.setX(0);  //stops front left motor
            BackRight.setX(0); //stops back right motor
            BackLeft.setX(0); //stops back left motor
        } catch (CANTimeoutException ex) {
            Scheduler.yield();
        }
    }
    public void balance(){
        
    }
    
}

class Vector{
    private double xComponent, yComponent, direction, magnitude;
    
    public Vector(double someMagnitude, double someDirection){
        magnitude = someMagnitude;
        direction = someDirection;
        xComponent = magnitude * Math.cos(direction);
        yComponent = magnitude * Math.sin(direction);
    }
    public double getXComponent(){
        return xComponent;
    }
    public double getYComponent(){
        return yComponent;
    }
    
    public double getDirection(){
        return direction;
    }
    public double getMagnitude(){
        return magnitude;
    }
}
